﻿using System;
using System.Linq;
using System.Collections.Generic;
using System.Text;
using System.Runtime.InteropServices;
using System.Drawing.Imaging;
using System.Drawing;
using MLRobotic.Tower.Common;

namespace MLRobotic.Tower.Detection
{
    public static class BitmapHelper
    {
        private static Log _log = new Log("BitmapHelper");

        public static void BufferToBitmap(byte[] buffer, Bitmap bitmap, PixelFormat pixelFormat)
        {
            _log.DebugStartWatch("BufferToBitmap");

            if (buffer.Length != bitmap.Width * bitmap.Height * GetBytesPerPixel(pixelFormat))
                throw new ArgumentException();

            BitmapData data = bitmap.LockBits(
                new Rectangle(0, 0, bitmap.Width, bitmap.Height),
                ImageLockMode.ReadWrite,
                pixelFormat);

            Marshal.Copy(buffer, 0, data.Scan0, buffer.Length);

            bitmap.UnlockBits(data);

            _log.DebugElapsed("BufferToBitmap");
        }

        public static void BitmapToBuffer(Bitmap bitmap, byte[] buffer, PixelFormat pixelFormat)
        {
            _log.DebugStartWatch("BitmapToBuffer");

            if (buffer.Length != bitmap.Width * bitmap.Height * GetBytesPerPixel(pixelFormat))
                throw new ArgumentException();

            BitmapData data = bitmap.LockBits(
                new Rectangle(0, 0, bitmap.Width, bitmap.Height),
                ImageLockMode.ReadOnly,
                pixelFormat);

            Marshal.Copy(data.Scan0, buffer, 0, buffer.Length);

            bitmap.UnlockBits(data);

            _log.DebugElapsed("BitmapToBuffer");
        }

        public static unsafe void FrameToBitmap(IntPtr frame, Bitmap bitmap, PixelFormat pixelFormat)
        {
            _log.DebugStartWatch("FrameToBitmap");

            BitmapData data = bitmap.LockBits(
                new Rectangle(0, 0, bitmap.Width, bitmap.Height),
                ImageLockMode.ReadWrite,
                pixelFormat);

            int bytesPerPixel = GetBytesPerPixel(data.PixelFormat);

            // convert the IntPtr to a ptr
            byte* src = (byte*)frame.ToPointer();
            byte* dest = (byte*)data.Scan0.ToPointer();

            for (int i = 0; i < data.Width * data.Height * bytesPerPixel; i++)
            {
                dest[i] = src[i];
            }


            bitmap.UnlockBits(data);

            _log.DebugElapsed("FrameToBitmap");
        }

        public static int GetBytesPerPixel(PixelFormat pixelFormat)
        {
            //_log.ResetElapsed();

            int result = 0;
            switch (pixelFormat)
            {
                case PixelFormat.Format16bppRgb555:
                    result = 2;
                    break;
                case PixelFormat.Format16bppRgb565:
                    result = 2;
                    break;
                case PixelFormat.Format24bppRgb:
                    result = 3;
                    break;
                case PixelFormat.Format32bppRgb:
                    result = 4;
                    break;
                default:
                    throw new NotImplementedException();
            }

            //_log.DebugElapsed("GetBytesPerPixel");

            return result;
        }

        public static void ComposeImages(Bitmap image, Bitmap mask, Color transparency)
        {
            //_log.DebugStartWatch("ComposeImages");

            using (Graphics graph = Graphics.FromImage(image))
            {
                ImageAttributes attributes = new ImageAttributes();
                attributes.SetColorKey(transparency, transparency);
                Rectangle dstRect = new Rectangle(0, 0, image.Width, image.Height);
                graph.DrawImage(mask, dstRect, 0, 0, image.Width, image.Height, GraphicsUnit.Pixel, attributes);
            }

            //_log.DebugElapsed("ComposeImages");
        }

        public static Bitmap CropImage(Bitmap image, Rectangle area)
        {
            //_log.DebugStartWatch("CropImage");

            var result = new Bitmap(area.Width, area.Height);

            using (Graphics graph = Graphics.FromImage(result))
            {
                Rectangle dstRect = new Rectangle(0, 0, area.Width, area.Height);
                graph.DrawImage(image, dstRect, area, GraphicsUnit.Pixel);
            }

            //_log.DebugElapsed("CropImage");

            return result;
        }

        public static unsafe void CopyAndInvertFrame(IntPtr source, byte[] dest, int width, int height, int bytePerPixel)
        {
            _log.DebugStartWatch("CopyAndInvertFrame");

            int stride = width * bytePerPixel;

            if (dest.Length != stride * height)
                throw new ArgumentException();

            byte* data = (byte*)source.ToPointer();
            int startRead = (height - 1) * stride;
            int startWrite = 0;

            for (int i = 0; i < height; ++i)
            {
                for (int j = 0; j < stride; j++)
                {
                    dest[startWrite + j] = data[startRead + j];
                }

                startRead -= stride;
                startWrite += stride;
            }

            _log.DebugElapsed("CopyAndInvertFrame");
        }

        public static Color CalculateMeanColor(byte[] buffer, Rectangle area, int width, int height, int bytesPerPixel)
        {
            _log.DebugStartWatch("CalculateMeanColor");

            int stride = width * bytesPerPixel;

            if (buffer.Length != stride * height)
                throw new ArgumentException();

            if (area.X + area.Width > width || area.Y + area.Height > height)
                throw new ArgumentException();

            int rSum = 0, gSum = 0, bSum = 0, nbSum = 0;

            for (int i = area.X; i < area.X + area.Width; i++)
            {
                for (int j = area.Y; j < area.Y + area.Height; j++)
                {
                    int index = j * stride + i * bytesPerPixel;

                    byte r, g, b;
                    BitmapHelper.Convert565ToRGB(buffer, index, out r, out g, out b);

                    rSum += r;
                    gSum += g;
                    bSum += b;
                    nbSum++;
                }
            }

            _log.DebugElapsed("CalculateMeanColor");

            return Color.FromArgb(rSum / nbSum, gSum / nbSum, bSum / nbSum);
        }
        
        public static void Convert565ToRGB(byte[] buffer, int index, out byte r, out byte g, out byte b)
        {
            Convert565ToRGB(buffer[index], buffer[index + 1], out r, out g, out b);
        }

        public static void Convert565ToRGB(byte low, byte high, out byte r, out byte g, out byte b)
        {
            //_log.ResetElapsed();

            r = (byte)((high >> 3) * 255 / 31);
            g = (byte)((((high << 3) & 0x38) | (low >> 5)) * 255 / 63);
            b = (byte)((low & 0x1F) * 255 / 31);

            //_log.DebugElapsed("Convert565ToRGB");
        }

        public static void ConvertRGBTo565(byte r, byte g, byte b, out byte low, out byte high)
        {
            //_log.ResetElapsed();

            high = (byte)(r & 0xF8 | g >> 5);
            low = (byte)(((g >> 2) << 5) | (b >> 3));

            //_log.DebugElapsed("ConvertRGBTo565");
        }

        public static void ConvertRGBTo565(byte[] buffer, int index, PixelFormat pixelFormat, out byte low, out byte high)
        {
            //_log.ResetElapsed();

            switch (pixelFormat)
            {
                //case PixelFormat.Format16bppRgb555:
                //    break;
                case PixelFormat.Format16bppRgb565:
                    low = buffer[index];
                    high = buffer[index + 1];
                    break;
                case PixelFormat.Format24bppRgb:
                    ConvertRGBTo565(buffer[index + 2], buffer[index + 1], buffer[index], out low, out high);
                    break;
                //case PixelFormat.Format32bppRgb:
                //    break;
                default:
                    throw new NotImplementedException();
            }

            //_log.DebugElapsed("ConvertRGBTo565");
        }

        public static void SetPixel(Color color, byte[] buffer, int index, PixelFormat pixelFormat)
        {
            //_log.ResetElapsed();

            switch (pixelFormat)
            {
                //case PixelFormat.Format16bppRgb555:
                //    break;
                case PixelFormat.Format16bppRgb565:
                    byte low, high;
                    ConvertRGBTo565(color.R, color.G, color.B, out low, out high);
                    buffer[index] = low;
                    buffer[index + 1] = high;
                    break;
                case PixelFormat.Format24bppRgb:
                    buffer[index + 2] = color.R;
                    buffer[index + 1] = color.G;
                    buffer[index] = color.B;
                    break;
                //case PixelFormat.Format32bppRgb:
                //    break;
                default:
                    throw new NotImplementedException();
            }

            //_log.DebugElapsed("SetPixel");
        }
    }
}
